package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.geometry.BasicGeometry;
import com.googlecode.curvedmetromaps.core.model.MetroLine;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S1: minimizes the spline curvature, by setting applying an angular
 * force between vertices.
 */
public class ForceAdjacentStationAngles extends ForceSubsystem {

	// Standard deviation of Gaussian kernel (parameter c). If 
	public ForceWeight stdDev = new ForceWeight("Standard deviation", 1);
	// Length of kernel is (kernelSize * stdDev * 2) + 1
	public ForceWeight kernelSize = new ForceWeight("Kernel size", 2);

	// Gaussian kernel
	private double[] kernelCache;
	private double kernelStdDev;

	ForceAdjacentStationAngles(ForceSystem system) {
		super(system);
		baseWeight.value = 20000;
	}

	void applyForces(int delta) {
		double[] kernel = getGaussianKernel(stdDev.value, kernelSize.value);
		
		double[][] angles = calculateAngles();

		double[][] blurredAngles = convoluteAngles(kernel, angles);

		// Calculate and apply forces
		for (MetroLine l : system.metroMap.lines) {
			for (int i = 0; i < l.stations.length - 1; i++) {
				if (i == 0 && l.isCycle == false)
					continue;
				
				int leftId = i >= 1 ? i - 1 : l.stations.length - 2;
				MetroStation m1 = l.stations[leftId]; // left child
				MetroStation m2 = l.stations[i]; // middle / root
				MetroStation m3 = l.stations[i + 1]; // right child
				
				Body b1 = system.stationBodies[m1.id];
				Body b2 = system.stationBodies[m2.id];
				Body b3 = system.stationBodies[m3.id];

				Vec2 p1 = b1.getPosition();
				Vec2 p2 = b2.getPosition();
				Vec2 p3 = b3.getPosition();

				Vec2 p21 = p1.sub(p2);
				Vec2 p23 = p3.sub(p2);

				double theta;
				if (l.isCycle) {
					// Line is a cycle, make it circular
					theta = angles[l.id][i] - (2 * Math.PI / (l.stations.length - 1));
				}
				else {
					// First rotate the vectors such that the vector from 2 to 1
					// lies on the x-axis. Then subtract Pi to set a straight line
					// (opposite vectors) to 0.
					theta = angles[l.id][i] - blurredAngles[l.id][i];
				}

				if (theta <= -Math.PI)
					theta += 2f * Math.PI;
				else if (theta >= Math.PI)
					theta -= 2f * Math.PI;

				Vec2 f1 = new Vec2(-p21.y, p21.x);
				Vec2 f3 = new Vec2(p23.y, -p23.x);

				float len1 = f1.normalize();
				float len3 = f3.normalize();

				float mag = (float) (theta * baseWeight.value);
				f1.mulLocal(mag / len1);
				f3.mulLocal(mag / len3);

				b1.applyForceToCenter(f1);
				b3.applyForceToCenter(f3);

				Vec2 f2 = f1.negateLocal().add(f3.negateLocal());
				b2.applyForceToCenter(f2);
			}
		}
	}

	/**
	 * Calculate angles between stations in metro lines.
	 *
	 * @return 2D array, 1st dimension are metro line IDs, 2nd dimension are station IDs. 
	 */
	private double[][] calculateAngles() {
		double[][] angles = new double[system.metroMap.lines.length][system.metroMap.maxLineLength];
		// Calculate angles between all consecutive stations triplets on a line.
		for (MetroLine l : system.metroMap.lines) {
			for (int i = 1; i < l.stations.length - 1; i++) {
				MetroStation m1 = l.stations[i - 1]; // left child
				MetroStation m2 = l.stations[i]; // middle / root
				MetroStation m3 = l.stations[i + 1]; // right child

				Vec2 p1 = system.stationBodies[m1.id].getPosition();
				Vec2 p2 = system.stationBodies[m2.id].getPosition();
				Vec2 p3 = system.stationBodies[m3.id].getPosition();

				angles[l.id][i] = BasicGeometry.dAngle(p1, p2, p3);
			}
			
			if (l.isCycle) {
				// Line is a cycle, thus first station = last station
				MetroStation m1 = l.stations[l.stations.length - 2]; // next to last station
				MetroStation m2 = l.stations[0]; // middle
				MetroStation m3 = l.stations[1]; // right child

				Vec2 p1 = system.stationBodies[m1.id].getPosition();
				Vec2 p2 = system.stationBodies[m2.id].getPosition();
				Vec2 p3 = system.stationBodies[m3.id].getPosition();

				angles[l.id][0] = BasicGeometry.dAngle(p1, p2, p3);
			}
		}
		return angles;
	}

	/**
	 * Apply a kernel to the angles (convolution), for example to blur the angles.
	 *
	 * @param kernel Kernel to apply.
	 * @param angles First dimension are metro lines, second dimension are metro stations.
	 * @return Convoluted angles.
	 */
	private double[][] convoluteAngles(double[] kernel, double[][] angles) {
		double[][] blurredAngles = new double[system.metroMap.lines.length][system.metroMap.maxLineLength];
		for (MetroLine l : system.metroMap.lines) {
			for (int i = 0; i < l.stations.length - 1; i++) {
				if (i == 0 && l.isCycle == false)
					continue;
				
				int r = (kernel.length - 1) / 2;

				int jStart = i - r;
				int jEnd = i + r;
				
				if (l.isCycle == false) {
					if (jStart < 1) {
						jStart = 1;
					}

					if (jEnd > l.stations.length - 2) {
						jEnd = l.stations.length - 2;
					}
				}

				double kernelSum = 0;
				for (int j = jStart; j <= jEnd; j++) {
					double g = kernel[j - i + r];
					
					// Index of angles array should wrap around it 
					int m = l.stations.length - 1;
					int jMod = j;
					if (l.isCycle) {
						jMod %= m;
						if (jMod < 0) jMod += m;
					}
					
					blurredAngles[l.id][i] += g * angles[l.id][jMod];
					kernelSum += g;
				}

				blurredAngles[l.id][i] /= kernelSum;
			}
		}
		return blurredAngles;
	}
	
	/**
	 * Get Gaussian kernel from cache, or create it if necessary.
	 */
	double[] getGaussianKernel(double stdDev, double stdDevWidth) {
		int m = (int) Math.round(stdDev * stdDevWidth);
		int len = (2 * m) + 1;
		
		// Create kernel if the cache is outdated.
		if (kernelCache == null || kernelCache.length != len || kernelStdDev != stdDev) {
			kernelCache = new double[len];
			// E^(-(x^2)/(2*c^2))
			double sum = 0;
			for (int i = 0; i <= m; i++) {
				double val = Math.exp(-(i * i) / (2 * stdDev * stdDev));
				kernelCache[m+i] = val;
				kernelCache[m-i] = val;
				sum += (i == 0 ? val : 2 * val); 
			}
			// Normalize: make sum equal to 1.
			for (int i = 0; i < len; i++) {
				kernelCache[i] /= sum;
			}
		}
		
		// Returned cached kernel
		return kernelCache;
	}
	
}
